
#include "cmd_func.h"
#include "stdlib.h"
#include "oslib_uart.h"
#include "FreeRTOS.h"
#include "task.h"
#include "aiming_cmd.h"
#include "shooter.h"

#include "cmd_claw.h"
#include "utils.h"
#include "rudder_chassis.h"

#ifdef OSLIB_CAN_MODULE_ENABLED
#include "oslib_can.h"
#endif

#if defined(OSLIB_UART_MODULE_ENABLED) && USE_OSLIB_UART_CLI
/* 在这里添加命令回调函数的定义或外部声明 */
static void Command_Hello(int argc, char *argv[])
{
    uprintf("Hello SimpleLib!\r\n");
}

static void Command_Echo(int argc, char *argv[])
{
    for (int i = 1; i < argc; i++)
        uprintf("Echo: %s\r\n", argv[i]);
}

#ifdef OSLIB_CAN_MODULE_ENABLED
static void Command_CanSend(int argc, char *argv[])
{
    if (argc >= 3)
    {
        can_msg msg;
        msg.in[0] = 0x40005850;
        msg.in[1] = 0x00000000;
        uint32_t id = strtol(argv[2], NULL, 16);
        if (argv[1][0] == 's')
        {
            can_send_msg(id, &msg);
            uprintf("CanSend:std[%x]\r\n", id);
        }
        else if (argv[1][0] == 'e')
        {
            can_ext_send_msg(id, &msg);
            uprintf("CanSend:ext[%x]\r\n", id);
        }
        else
            uprintf("Param Error!\r\n");
    }
    else
    {
        uprintf("Param Error!\r\n");
    }
}
#endif

#if ((configUSE_TRACE_FACILITY == 1) && (configUSE_STATS_FORMATTING_FUNCTIONS > 0) && (configSUPPORT_DYNAMIC_ALLOCATION == 1))
static char tasklist_buffer[256];
static void Command_Task(int argc, char *argv[])
{
    uprintf("Free Heap: %dB\r\n", xPortGetFreeHeapSize());
    uprintf("Name          \tState\tPrio\tStack\tRank\r\n");
    vTaskList(tasklist_buffer);
    uprintf(tasklist_buffer);
    // uprintf("Name          \tCount\tUtilization\r\n");
    // vTaskGetRunTimeStats(tasklist_buffer);
    // uprintf(tasklist_buffer);
}
#endif

static void Command_ShootStart(int argc, char *argv[])
{
    // if (0 != shooter_test_start(shooter1)) {
    //     uprintf("Failed to set start\n");
    // }
    shooter_restart(shooter1);
}

static void Command_ShootPause(int argc, char *argv[])
{
    shooter_pause(shooter1);
}

static void Command_ShootReady(int argc, char *argv[])
{
    if (0 != shooter_load_ready(shooter1))
    {
        uprintf("Failed to set ready\n");
    }
}

static void Command_ShootAim(int argc, char *argv[])
{
    if (0 != shooter_start_aim(shooter1))
    {
        uprintf("Failed to start aim\n");
    }
}

static void Command_ShootSetParam(int argc, char *argv[])
{
    //TODO: 设定参数
    if (argc == 3)
    {
        int tar_force = atoi(argv[1]);
        int tar_pitch = atoi(argv[2]);
        if (tar_force <= 0 || tar_pitch > 0)
        {
            uprintf("Failed to set target force.\r\n");
            return;
        }
        if (osOK == shooter_set_param(shooter1, tar_force, tar_pitch))
        {
            UDebug("Command_ShootSetParam OK");
            uprintf("Target: f=%d, p=%d\r\n", tar_force, tar_pitch);
        }
        else
        {
            UDebug("Command_ShootSetParam Failed");
            uprintf("Failed to set target force.\r\n");
        }
    }
    else
    {
        uprintf("Usage: setp <target force> <target pitch>\r\n");
    }
    UDebug("Command_ShootSetParam Over");
}

static void Command_ShootShoot(int argc, char *argv[])
{
    if (0 != shooter_test_shoot(shooter1))
    {
        uprintf("Failed to set shoot\n");
    }
}

static void Command_ShootReadYaw(int argc, char *argv[])
{
    shooter_read_yaw(shooter1);
}

/********************************[底盘控制]***********************************/
extern float CMD_TargetSpeed;
extern float CMD_TargetDir;
extern float CMD_TargetOmega;
void CMD_Chassis_Teleop_Acc(int argc, char **argv)
{
    CMD_TargetSpeed += 0.05;
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_Dec(int argc, char **argv)
{
    CMD_TargetSpeed -= 0.05;
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_GoAhead(int argc, char **argv)
{
    CMD_TargetOmega = 0;
    CMD_TargetSpeed += 0.01;
    CMD_TargetDir = 1.5708;
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_GoBack(int argc, char **argv)
{
    CMD_TargetOmega = 0;
    CMD_TargetSpeed -= 0.01;
    CMD_TargetDir = 1.5708;
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_TurnLeft(int argc, char **argv)
{
    CMD_TargetOmega = 0.5;
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_TurnRight(int argc, char **argv)
{
    CMD_TargetOmega = -0.5;
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_Stop(int argc, char **argv)
{
    CMD_TargetSpeed = 0;
    CMD_TargetOmega = 0;
    RudderChassis.DriveMotors.motor_mode = MTR_CTRL_RPM;
    for (int i = 0; i < 4; i++)
    {
        RudderChassis.DriveMotors.target_rpm[i] = 0;
    }
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_Teleop_ShiftRight(int argc, char **argv)
{
    CMD_TargetDir -= __ANGLE2RAD(10);
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}
void CMD_Chassis_Teleop_ShiftLeft(int argc, char **argv)
{
    CMD_TargetDir += __ANGLE2RAD(10);
    uprintf("--Vel:%.2f, Dir:%.2f, Omega:%.2f\r\n", CMD_TargetSpeed, CMD_TargetDir, CMD_TargetOmega);
}

void CMD_Chassis_SetTargetPoint(int argc, char **argv)
{
    if (argc == 5)
    {
        BaseChassis.Go2PointStatus.start_speed = atof(argv[4]);
    }
    BaseChassis.Go2PointStatus.target_point.x = atof(argv[1]);
    BaseChassis.Go2PointStatus.target_point.y = atof(argv[2]);
    BaseChassis.Go2PointStatus.target_yaw = atof(argv[3]);
    BaseChassis.Go2PointStatus.final_speed = 0;
    BaseChassis.Go2PointStatus.start = 1;
    BaseChassis.Go2PointStatus.enable = 1;
    uprintf("CMD|Target point set to (%.2f,%.2f,%.2f,%.2f)\r\n",
            BaseChassis.Go2PointStatus.target_point.x,
            BaseChassis.Go2PointStatus.target_point.y,
            BaseChassis.Go2PointStatus.target_yaw, BaseChassis.Go2PointStatus.start_speed);
}

/**
 * @brief 打印底盘信息
 * 
 * @note 配合全局变量Chassis_PrintPostureStatus_Flag在main函数中调用
 */
void CMD_SwitchPrintChassisStatus(int argc, char *argv[])
{
    extern uint8_t Chassis_PrintPostureStatus_Flag;
    Chassis_PrintPostureStatus_Flag = (Chassis_PrintPostureStatus_Flag + 1) % 2;
    if (Chassis_PrintPostureStatus_Flag)
    {
        uprintf("--Start print chassis status!\r\n");
    }
    else
    {
        uprintf("--Stop print chassis status!\r\n");
    }
}

void CMD_SwitchPrintTargetStatus(int argc, char **argv)
{
    extern int PrintTargetStatus_Flag;
    PrintTargetStatus_Flag = (PrintTargetStatus_Flag + 1) % 2;
    if (PrintTargetStatus_Flag == 1)
    {
        uprintf("--Start print target status!\r\n");
    }
    else
    {
        uprintf("--Stop print target status!\r\n");
    }
}

void CMD_ChangePosMode(int argc, char **argv)
{
    BaseChassis.pos_mode = (BaseChassis.pos_mode + 1) % 2;
    if (BaseChassis.pos_mode == POS_MODE_ABSOLUTE)
    {
        uprintf("--CMD|Pos mode change to Absolute!\r\n");
    }
    else if (BaseChassis.pos_mode == POS_MODE_RELATIVE)
    {
        uprintf("--CMD|Pos mode change to Relative!\r\n");
    }
}

void CMD_ChangeCtrlMode(int argc, char **argv)
{
    BaseChassis.ctrl_mode = (BaseChassis.ctrl_mode + 1) % 5;
    switch (BaseChassis.ctrl_mode)
    {
    case CTRL_MODE_NONE:
        uprintf("CMD|Ctrl mode change to none...\r\n");
        break;
    case CTRL_MODE_HANDLE:
        uprintf("CMD|Ctrl mode change to handle...\r\n");
        break;
    case CTRL_MODE_CMD:
        uprintf("CMD|Ctrl mode change to cmd...\r\n");
        break;
    case CTRL_MODE_TRACK:
        uprintf("CMD|Ctrl mode change to track...\r\n");
        break;
    case CTRL_MODE_GO_TO_POINT:
        uprintf("CMD|Ctrl mode change to go2point...\r\n");
        break;
    }
    RudderChassis.base->handbrake_flag = 0;
}

extern float CMD_TargetYaw;
extern char YawTuning_Start;
void CMD_YawTuning(int argc, char **argv)
{
    BaseChassis.ctrl_mode = CTRL_MODE_TUNING;
    CMD_TargetYaw = __ANGLE2RAD(atof(argv[1]));
    extern PID_s YawPID;
    float kp = atof(argv[2]);
    float kd = atof(argv[3]);
    float ki = atof(argv[4]);
    float int_duty = atof(argv[5]);
    YawTuning_Start = atoi(argv[6]);
    YawPID.Kp = kp;
    YawPID.Kd = kd;
    YawPID.Ki = ki;
    YawPID.int_duty = int_duty;
    uprintf("--CMD|TargetYaw:%.2f kp:%.2f kd:%.2f ki:%.2f int_duty:%.2f start_flag:%d \r\n",
            CMD_TargetYaw, kp, kd, ki, int_duty, YawTuning_Start);
}

/**
 * @brief 设置偏航角PID的参数
 */
void CMD_SetYawPID(int argc, char **argv)
{
    extern PID_s YawPID;
    float kp = atof(argv[1]);
    float kd = atof(argv[2]);
    float ki = atof(argv[3]);
    float int_duty = atof(argv[4]);
    YawPID.Kp = kp;
    YawPID.Kd = kd;
    YawPID.Ki = ki;
    YawPID.int_duty = int_duty;
    uprintf("--CMD|YawPID - kp:%.2f kd:%.2f ki:%.2f int_duty:%.2f \r\n",
            kp, kd, ki, int_duty);
}

void CMD_SetDriveMotorsCurr(int argc, char **argv)
{
    RudderChassis.DriveMotors.target_curr[0] = atof(argv[1]);
    RudderChassis.DriveMotors.target_curr[1] = atof(argv[2]);
    RudderChassis.DriveMotors.target_curr[2] = atof(argv[3]);
    RudderChassis.DriveMotors.target_curr[3] = atof(argv[4]);
    uprintf("--CMD|Curr:[0]:%.2f [1]:%.2f [2]:%.2f [3]:%.2f\r\n",
            RudderChassis.DriveMotors.target_curr[0],
            RudderChassis.DriveMotors.target_curr[1],
            RudderChassis.DriveMotors.target_curr[2],
            RudderChassis.DriveMotors.target_curr[3]);
}

void CMD_SetDriveMotorsDuty(int argc, char **argv)
{
    RudderChassis.DriveMotors.target_duty[0] = atof(argv[1]);
    RudderChassis.DriveMotors.target_duty[1] = atof(argv[2]);
    RudderChassis.DriveMotors.target_duty[2] = atof(argv[3]);
    RudderChassis.DriveMotors.target_duty[3] = atof(argv[4]);
    uprintf("--CMD|duty:[0]:%.2f [1]:%.2f [2]:%.2f [3]:%.2f\r\n",
            RudderChassis.DriveMotors.target_duty[0],
            RudderChassis.DriveMotors.target_duty[1],
            RudderChassis.DriveMotors.target_duty[2],
            RudderChassis.DriveMotors.target_duty[3]);
}

/**
 * @brief 设置法向修正向量的PID参数
 */
void CMD_SetNormalCorrPID(int argc, char **argv)
{
    extern PID_s NormalCorrPID_x, NormalCorrPID_y;
    float kp = atof(argv[1]);
    float ki = atof(argv[2]);
    float kd = atof(argv[3]);
    float int_duty = atof(argv[4]);
    NormalCorrPID_x.Kp = kp;
    NormalCorrPID_x.Kd = kd;
    NormalCorrPID_x.Ki = ki;
    NormalCorrPID_x.int_duty = int_duty;
    NormalCorrPID_y.Kp = kp;
    NormalCorrPID_y.Kd = kd;
    NormalCorrPID_y.Ki = ki;
    NormalCorrPID_y.int_duty = int_duty;
    uprintf("--CMD|NormalCorrPID - kp:%.2f ki:%.2f kd:%.2f int_duty:%.2f \r\n",
            kp, ki, kd, int_duty);
}

/**
 * @brief 设置原地锁定PID参数 
 */
void CMD_SetLockPID(int argc, char **argv)
{
    extern PID_s LockPID;
    float kp = atof(argv[1]);
    float ki = atof(argv[2]);
    float kd = atof(argv[3]);
    float int_duty = atof(argv[4]);
    LockPID.Kp = kp;
    LockPID.Kd = kd;
    LockPID.Ki = ki;
    LockPID.int_duty = int_duty;
    uprintf("--CMD|LockPID - kp:%.2f ki:%.2f kd:%.2f i_duty:%.2f\r\n",
            kp, ki, kd, int_duty);
}

void CMD_SwitchPrintMotorStatus(int argc, char **argv)
{
    extern uint8_t SW_PrintMotorStatus_Flag;
    SW_PrintMotorStatus_Flag = (SW_PrintMotorStatus_Flag + 1) % 2;
    if (SW_PrintMotorStatus_Flag == 1)
    {
        uprintf("--Start print motor status!\r\n");
    }
    else
    {
        uprintf("--Stop print motor status!\r\n");
    }
}

/* 在这里完成命令到回调函数的映射 */
void Command_Init()
{
    OSLIB_UartCmd_AddCommand("hello", "\"Hello SimpleLib!\"", Command_Hello); /* 输出 "Hello SimpleLib!" */ // 测试串口输出和命令
    OSLIB_UartCmd_AddCommand("echo", "Echo message", Command_Echo); /* 回显参数的内容 */                    // 测试命令参数解析
#ifdef OSLIB_CAN_MODULE_ENABLED
    OSLIB_UartCmd_AddCommand("cansend", "CAN send message", Command_CanSend); /* 发送CAN报文, 内容为两个整数 */ // 测试CAN发送
#endif
#if ((configUSE_TRACE_FACILITY == 1) && (configUSE_STATS_FORMATTING_FUNCTIONS > 0) && (configSUPPORT_DYNAMIC_ALLOCATION == 1))
    OSLIB_UartCmd_AddCommand("tasks", "Show task list", Command_Task);
#endif
    /* 调试云台 */
    OSLIB_UartCmd_AddCommand("autoaiming", "autoaiming", setAutoAiming);
    OSLIB_UartCmd_AddCommand("manualaiming", "manualaiming", setManualAiming);
    OSLIB_UartCmd_AddCommand("setid", "setAimingId <id>", setAimingId);
    OSLIB_UartCmd_AddCommand("homing", "homing <num> <vel> <cur>", commandHoming);
    OSLIB_UartCmd_AddCommand("right", "change right", commandTestRight);
    OSLIB_UartCmd_AddCommand("left", "change left", commandTestLeft);
    OSLIB_UartCmd_AddCommand("running", "boxreader running state", commandRunning);
    OSLIB_UartCmd_AddCommand("pidm", "Modify PID and imax", commandPID);
    
    /* 调试射箭 */
    OSLIB_UartCmd_AddCommand("start", "Start Continuous Shooting", Command_ShootStart);
    OSLIB_UartCmd_AddCommand("pause", "Pause Continuous Shooting", Command_ShootPause);
    OSLIB_UartCmd_AddCommand("ready", "Load Done", Command_ShootReady);
    OSLIB_UartCmd_AddCommand("aim", " Start Aim", Command_ShootAim);
    OSLIB_UartCmd_AddCommand("arrive", "Manual Arrive", commandManualArrive);
    OSLIB_UartCmd_AddCommand("setp", "Set Param <target force> <target pitch>", Command_ShootSetParam);
    OSLIB_UartCmd_AddCommand("shoot", "Shooter shoot", Command_ShootShoot);
    OSLIB_UartCmd_AddCommand("readyaw", "Shooter read yaw value", Command_ShootReadYaw);

    /*测试抓手*/
    OSLIB_UartCmd_AddCommand("ClawCtr", "Claw state <first> <second> <third>", Command_ClawCtr);
    OSLIB_UartCmd_AddCommand("MotorInit", "Position return to zero", Command_MotorInit);
    OSLIB_UartCmd_AddCommand("PosCtr", "Move claws <position>", Command_PosCtr);
    OSLIB_UartCmd_AddCommand("MotorOff", "Move motor off", Command_MotorOff);
    OSLIB_UartCmd_AddCommand("TestHoming", "Test claw move homing", Command_TestHoming);
    /*指令控制抓手状态机*/
    OSLIB_UartCmd_AddCommand("OpenClawMachine", "open claw machine", Command_OpenClawMachine);
    OSLIB_UartCmd_AddCommand("CloseClawMachine", "close claw machine", Command_CloseClawMachine);
    OSLIB_UartCmd_AddCommand("StartTakeArrow", "start taking arrows", Command_StartTakeArrow);    //开始取箭
    OSLIB_UartCmd_AddCommand("MoveClaw", "move claw to prepare another arrow", Command_MoveClaw); //左右移动
    OSLIB_UartCmd_AddCommand("OpenClaw", "OpenClaw to place arrow", Command_OpenClaw);            //松手放箭

    // Chassis tuning
    OSLIB_UartCmd_AddCommand("ChangePosMode", "", CMD_ChangePosMode);
    OSLIB_UartCmd_AddCommand("ChangeCtrlMode", "", CMD_ChangeCtrlMode);
    OSLIB_UartCmd_AddCommand("SwitchPrintTargetStatus", "press to change ", CMD_SwitchPrintTargetStatus);
    OSLIB_UartCmd_AddCommand("SwitchPrintChassisStatus", "", CMD_SwitchPrintChassisStatus);
    OSLIB_UartCmd_AddCommand("YawTuning", "<TargetYaw><kp><kd><ki><intTime><StartFlag>", CMD_YawTuning);
    OSLIB_UartCmd_AddCommand("SetYawPID", "<kp><kd><ki><intTime>", CMD_SetYawPID);
    OSLIB_UartCmd_AddCommand("SetNormalCorrPID", "<kp><kd><ki><intTime>", CMD_SetNormalCorrPID);
    OSLIB_UartCmd_AddCommand("SetDriveMotorCurr", "", CMD_SetDriveMotorsCurr);
    OSLIB_UartCmd_AddCommand("SetDriveMotorDuty", "", CMD_SetDriveMotorsDuty);
    OSLIB_UartCmd_AddCommand("SetTargetPoint", "<x><y>", CMD_Chassis_SetTargetPoint);
    OSLIB_UartCmd_AddCommand("SetLockPID", "", CMD_SetLockPID);

    // teleop
    OSLIB_UartCmd_AddCommand("Teleop_Acc", "", CMD_Chassis_Teleop_Acc);
    OSLIB_UartCmd_AddCommand("Teleop_Dec", "", CMD_Chassis_Teleop_Dec);
    OSLIB_UartCmd_AddCommand("Teleop_GoAhead", "", CMD_Chassis_Teleop_GoAhead);
    OSLIB_UartCmd_AddCommand("Teleop_GoBack", "", CMD_Chassis_Teleop_GoBack);
    OSLIB_UartCmd_AddCommand("Teleop_TurnLeft", "", CMD_Chassis_Teleop_TurnLeft);
    OSLIB_UartCmd_AddCommand("Teleop_TurnRight", "", CMD_Chassis_Teleop_TurnRight);
    OSLIB_UartCmd_AddCommand("Teleop_ShiftLeft", "", CMD_Chassis_Teleop_ShiftLeft);
    OSLIB_UartCmd_AddCommand("Teleop_ShiftRight", "", CMD_Chassis_Teleop_ShiftRight);
    OSLIB_UartCmd_AddCommand("Teleop_Stop", "", CMD_Chassis_Teleop_Stop);
}

#endif